# 内置库 
import math
import os
import sys
import importlib
import json
import time

# 第三方库
import numpy as np

kdtree = importlib.import_module("onsite-unstructured.common.kdtree")

def point_in_circle(x, y, x0, y0, r):
    distance = math.sqrt((x - x0)**2 + (y - y0)**2)
    if distance <= r:
        return True
    else:
        return False

def min_distance_to_border(observation, target_x, target_y):
    min_distance = float('inf')  # 初始化最小距离为无穷大

    for i in range(len(observation["hdmaps_info"]["tgsc_map"].borderline)):
        borderpoints = observation["hdmaps_info"]["tgsc_map"].borderline[i]['borderpoints']
        for point in borderpoints:
            border_x, border_y = point[0], point[1]
            distance = math.sqrt((target_x - border_x)**2 + (target_y - border_y)**2)
            min_distance = min(min_distance, distance)

    return min_distance

class Point_t:
    """2维k-d tree的基础数据结构,KDNode."""

    def __init__(self, x: float = None, y: float = None, ind: int = None,vel: float = None):
        """_summary_

        Args:
            x (float, optional): 笛卡尔坐标系x. Defaults to None.
            y (float, optional): 笛卡尔坐标系y. Defaults to None.
            ind (int, optional): 在data中的结果索引inddex. Defaults to None.
        """
        self.x = x
        self.y = y
        self.ind = ind
        self.vel = vel


class Item(object):
    def __init__(self, x, y, data):
        self.coords = (x, y)
        self.data = data

    def __len__(self):
        return len(self.coords)

    def __getitem__(self, i):
        return self.coords[i]

    def __repr__(self):
        return "Item({},{},{})".format(self.coords[0], self.coords[1], self.data)

def get_xy_coordinates(traj):
    xy_list = []
    for time_point, data in traj.items():
        if isinstance(time_point, float) or (isinstance(time_point, str) and time_point.replace('.', '', 1).isdigit()):
            x = data.get('x')
            y = data.get('y')
            xy_list.append((x, y))
    return xy_list

def find_closest_point_index_and_distance(trajectory, ego_x, ego_y):
    min_distance = float('inf')
    min_index = -1
    for i, (x, y) in enumerate(trajectory):
        distance = math.sqrt((x - ego_x) ** 2 + (y - ego_y) ** 2)
        if distance < min_distance:
            min_distance = distance
            min_index = i
    return min_index, min_distance

dir_current_file = os.path.dirname(__file__)  # 'Algorithm_demo_1\planner'
dir_parent_1 = os.path.dirname(dir_current_file)  # 'Algorithm_demo_1'

class MotionController:
    """运动控制模块.
    功能设计:跟踪规划模块输入的路径曲线,速度曲线;
    """

    def __init__(self,min_prev_dist):
        self._min_prev_dist = min_prev_dist  # 最小预瞄距离
        self._prev_k = 0.5  # 预瞄距离速度系数
        self.L = 5.6  # 轴距
        self._is_create_kdtree = False
        self._last_acc = -999

    def process(self, vehicle_info, path_waypoints, spd_planned, observation,Reverse,traj,scenario_to_test,switch):
        """控制主函数
        输入:_a_max自车最大加速度,_vel自车当前车速,_vel0自车期望车速,delta加速度指数,
        _dv自车与前车速度差,_s自车与前车车距,_b舒适减速度,_t安全时距,_s0最小车距
        """
        #################更新参数#################
        self._ego_v = vehicle_info["ego"]["v_mps"]
        self._ego_x = vehicle_info["ego"]["x"]
        self._ego_y = vehicle_info["ego"]["y"]
        self._ego_yaw = vehicle_info["ego"]["yaw_rad"]
        self._ego_shape = vehicle_info["ego"]["shape"]
        self._vehicle_info = vehicle_info
        self.L = (vehicle_info["ego"]["shape"]["length"]/1.7)  #对两种车型轴距进行自适应#
        #################只创建一次kdtree#################
        if self._is_create_kdtree == False:
            path_tuple = list()
            i = 0  # 记录路径点下标
            for waypoint in path_waypoints:
                point = Item(waypoint[0], waypoint[1], str(i))  # ! (x,y,index) 用于构建KD-tree
                path_tuple.append(point)
                i += 1
            self._tree = kdtree.create(path_tuple)
        #################匹配与车辆最近的路径点#################
        nearest_tree_node = self._tree.search_nn([self._ego_x, self._ego_y])
        self._nearest_pt_id = int(nearest_tree_node[0].data.data)  # 最近路径点的下标
        self._nearest_pt = path_waypoints[self._nearest_pt_id]
        #################计算预瞄距离#################
        prev_dist = self._min_prev_dist + self._prev_k * self._ego_v

        try:
            if path_waypoints[self._nearest_pt_id+200][3] == False:
                switch = True
        except:
            if path_waypoints[self._nearest_pt_id][3] == False:
                switch = True  
        #################寻找预瞄点#################
        prev_pt = Point_t(0, 0, -10000,0)
        if len(path_waypoints) == 0:
            print("###log### PATH EMPTY!!!")

        scene_name = scenario_to_test['data']['scene_name']
        json_file = os.path.join(dir_parent_1, "inputs", "Scenarios", f"Scenario-{scene_name}.json")  # 构造JSON文件路径

        with open(json_file, "r") as f:  # 加载JSON文件
            scenario_data = json.load(f)

        max_t = scenario_data["max_t"]
        start_x = scenario_data["ego_info"]["start_states"]["x"]
        start_y = scenario_data["ego_info"]["start_states"]["y"]

        closest_index, min_distance = float('inf'), float('inf')
        if 1 in traj:
            # 获取轨迹的 xy 坐标列表
            trajectory = get_xy_coordinates(traj[1])

            # 找到离起点最近的点在轨迹中的索引和最小距离
            closest_index, min_distance = find_closest_point_index_and_distance(trajectory,start_x,start_y)
        if len(path_waypoints) >= 1761 and max_t <= 55:
            a1 = 4
            v = 12.5
        elif min_distance<10 and closest_index<400:   # 40s内会有背景车撞到起点
            # print(f"closest_index:{closest_index}, min_distance:{min_distance}")
            a1 = 5
            v = 12.5
        else:
            a1 = 4
            v = 11.3
        for i in range(-1, -len(path_waypoints) - 1, -1):
            temp_dist = math.hypot(path_waypoints[i][0] - self._ego_x, path_waypoints[i][1] - self._ego_y)
            if temp_dist < prev_dist:
                prev_pt.x = path_waypoints[i][0]
                prev_pt.y = path_waypoints[i][1]
                prev_pt.ind = 0
                prev_pt.vel= spd_planned[i]
                # print(i)
                break
        # print(i)
        if prev_pt.ind == -10000:
            Reverse = False
            print("CAN NOT FIND PREVIEW POINT!!! 切换控制方法为LQR")
        #################纯跟踪输出前轮转角#################
        delta = self._purePursuit(prev_pt)  # 根据给定的前一个路径点 prev_pt 计算纯跟踪控制的前轮转角 delta

        # if observation["test_setting"]["scenario_type"] == "loading":
        #     v = spd_planned[self._nearest_pt_id]
        acc = self._IDM(a1, self._ego_v, v, 100.0, 0.0, 9999.0, 1.0, 3.5, 31.5,Reverse)

        if self._last_acc != -999:  # 平滑加速度的变化
            #     if acc - self._last_acc > 1.0:
            #         acc = self._last_acc + 1  # 如果两者之差大于 1.0，则将当前加速度修改为上一次加速度加上 1
            if acc - self._last_acc < -1.0:
                acc = self._last_acc - 1  # 如果两者之差小于 -1.0，则将当前加速度修改为上一次加速度减去 1
        self._last_acc = acc
        min_dist = min_distance_to_border(observation, np.mean(observation['test_setting']['goal']['x']),
                                     np.mean(observation['test_setting']['goal']['y']))
        if min_dist <= 9:
            r = 20
        else:
            r = 2
        if 'new_goal' not in observation['test_setting']:
            arrive = point_in_circle(vehicle_info['ego']['x'], vehicle_info['ego']['y'], np.mean(observation['test_setting']['goal']['x']),
                                     np.mean(observation['test_setting']['goal']['y']), r)
        else:
            arrive = point_in_circle(vehicle_info['ego']['x'], vehicle_info['ego']['y'],
                                     np.mean(observation['test_setting']['new_goal']['x']),
                                     np.mean(observation['test_setting']['new_goal']['y']), r)
        return (acc, delta, 1), i, Reverse,arrive,switch

    def _IDM(self, a_max, vel, vel0, delta, dv, s, b, t, s0,Reverse):
        """智能驾驶模型,计算加速度.[纵向跟踪控制模块]
        输入:_a_max自车最大加速度,_vel自车当前车速,_vel0自车期望车速,delta加速度指数,
        _dv自车与前车速度差,_s自车与前车车距,_b舒适减速度,_t安全时距,_s0最小车距
        """
        expected_s = vel * t + vel * dv / (2 * math.sqrt(a_max * b))
        expected_s = s0 + max(0.0, expected_s)
        temp_a = pow(vel / vel0, delta)
        temp_b = pow(expected_s / s, 2)
        if Reverse==True:
            a_max = 5
            temp_a = 2
            temp_b = 1
        elif (vel <= 0 and Reverse==False):
            a_max = 1
            temp_a = 0
            temp_b = 0

        return a_max * (1 - temp_a - temp_b)

    def _purePursuit(self, prev_pt):
        """纯跟踪算法.[横向跟踪控制模块]
        输入:prev_pt——预瞄点
        输出:delta——前轮转角
        """
        lat_d = (prev_pt.y - self._ego_y) * math.cos(self._ego_yaw) - (prev_pt.x - self._ego_x) * math.sin(
            self._ego_yaw)  # 横向偏差
        Ld = math.hypot(prev_pt.y - self._ego_y, prev_pt.x - self._ego_x)  # 车与预瞄点距离
        delta = math.atan(2 * self.L * lat_d / (Ld ** 2))  # 前轮转角
        return delta
